import numpy as np


def initialize_calibration(calibration_file_path):
    """
    从calibrate.txt文件加载相机内参。
    假设文件格式是每行包含 fx fy cx cy。
    如果有多组参数，我们只取第一组。
    """
    try:
        with open(calibration_file_path, 'r') as f:
            lines = f.readlines()
            # 找到包含四个数字的行
            for line in lines:
                parts = line.strip().split()
                if len(parts) >= 4:
                    try:
                        fx = float(parts[0])
                        fy = float(parts[1])
                        cx = float(parts[2])
                        cy = float(parts[3])

                        K = np.array([[fx, 0, cx],
                                      [0, fy, cy],
                                      [0, 0, 1]])
                        # 假设图像尺寸是根据cx,cy推断的，或者你手动给出
                        # 常见的相机中心是图像宽度/2，高度/2
                        image_width = int(cx * 2)
                        image_height = int(cy * 2)

                        print(f"Loaded Camera Intrinsics: fx={fx}, fy={fy}, cx={cx}, cy={cy}")
                        print(f"Assumed Image Resolution: {image_width}x{image_height}")
                        return K, image_width, image_height
                    except ValueError:
                        continue  # 如果转换失败，尝试下一行
        print("Error: Could not find valid camera intrinsic parameters in the calibration file.")
        return None, 0, 0
    except FileNotFoundError:
        print(f"Error: Calibration file not found at {calibration_file_path}")
        return None, 0, 0


if __name__ == '__main__':
    # 示例用法
    # 请将 'path/to/your/calibrate.txt' 替换为实际路径
    K_matrix, img_w, img_h = initialize_calibration('../config/calibrate.txt')
    if K_matrix is not None:
        print("\nIntrinsic Matrix (K):\n", K_matrix)
        print("Image Width:", img_w)
        print("Image Height:", img_h)

